# -*- coding = utf-8 -*-
# @Time : 2023/3/15 17:12
# @Author : 龙王赘婿_彪少
# @File : radar_decode.py
# @Software: PyCharm

# 功能：
import math

# 目标列表头信息 0x60A
def decode_head(data):
    Objects_NofObjects = data[0]
    Objects_MeasCount = (data[1] << 8) + data[2]
    Objects_InterfaceVersion = (data[3] >> 4)
    print("*" * 100)
    print(f"Objects_NofObjects (目标ID) = {Objects_NofObjects}")
    print(f"Objects_MeasCount (循环次数) = {Objects_MeasCount}")
    print(f"Objects_InterfaceVersion (CAN版本接口) = {Objects_InterfaceVersion}")
    print("*" * 100)


# 目标通用信息解算 0x60B
def decode_body(data):
    Objects_ID = data[0]
    Objects_DistLong = ((data[1] << 5) + ((data[2] >> 3) & 0x1f)) * 0.2 - 500
    Objects_DistLat = (((data[2] & 0x07) << 8) + data[3]) / 5.0 - 204.6
    Objects_VrelLong = ((data[4] << 2) + ((data[5] >> 6) & 0x03)) * 0.25 - 128
    Objects_VrelLat = (((data[5] & 0x3f) << 3) + ((data[6] >> 5)& 0x07)) * 0.25 - 64
    Objects_DynProp = (data[6]) & 0x07
    Objects_Class = (data[6] & 0x18) >> 3
    Objects_RCS = data[7] * 0.5 - 64
    angle = math.atan(Objects_DistLat / Objects_DistLong) * 180 / math.pi
    distance = math.sqrt(Objects_DistLong * Objects_DistLong + Objects_DistLat * Objects_DistLat)
    print("*" * 100)
    print(f"Objects_ID (目标ID) = {Objects_ID}")
    print(f"Objects_DistLong (纵向距离) = {Objects_DistLong}")
    print(f"Objects_DistLat (横向距离) = {Objects_DistLat}")
    print(f"Objects_VrelLong (纵向速度) = {Objects_VrelLong}")
    print(f"Objects_VrelLat (横向速度) = {Objects_VrelLat}")
    print(f"Objects_DynProp (动态属性) = {Objects_DynProp}")
    print(f"Objects_Class (无意义) = {Objects_Class}")
    print(f"Objects_RCS (RCS) = {Objects_RCS}")
    print(f"angle = {angle}")
    print(f"distance = {distance}")
    print("*" * 100)

if __name__ == "__main__":
    # data = [0x01, 0x4F, 0x63, 0xFD, 0x80, 0x20, 0x08, 0x98]

    head = [0x04, 0x03, 0x38, 0x00, 0x01, 0x00, 0x00, 0x00]
    data1 = [0x03, 0x4E, 0x63, 0xFC, 0x80, 0x20, 0x08, 0x98]
    data2 = [0x00, 0x4E, 0xAC, 0x04, 0x80, 0x20, 0x08, 0x90]
    data3 = [0x02, 0x4E, 0xC4, 0x02, 0x80, 0x20, 0x08, 0x90]
    data4 = [0x05, 0x4E, 0xF3, 0xFF, 0x80, 0x20, 0x08, 0x98]

    decode_head(head)

    decode_body(data1)
    decode_body(data2)
    decode_body(data3)
    decode_body(data4)

